#include "evaluate/map_evaluate_adapter.h"

#include <boost/json.hpp>
#include <fstream>
#include <iostream>

#include "common/map_info.h"
#include "common/map_utility_impl.h"

namespace autodrive {
namespace perception {

void write_matrix(Eigen::MatrixXd const &matrix,
                  std::filesystem::path const &file_path) noexcept(false) {
  Eigen::IOFormat txtFormat(Eigen::FullPrecision, Eigen::DontAlignCols, " ",
                            "\n");
  std::ofstream file(file_path);
  if (!file.is_open()) {
    throw std::runtime_error("file " + file_path.string() + " can not open");
  }
  file << matrix.format(txtFormat);
  file.close();
}

std::int32_t run_map_decode_and_post(char const *settings,
                                     MapHeadDataRaw const &data,
                                     double const *pose, char *map, char *err) {
  std::int32_t iRet{-1};
  std::string errMsg{""};

  try {
    if (err == nullptr) {
      errMsg = "err is nullptr";
      std::cout << "[" << "\033[1;5;31m" << "Error" << "\033[0m" << "] "
                << "run_map_decode_and_post error, " << errMsg << std::endl;
      return iRet;
    }

    auto &mapEvaluateAdapter{MapEvaluateAdapter::GetInstance()};
    mapEvaluateAdapter.Init(settings);

    if (mapEvaluateAdapter.IsMapFilterEnable()) {
      if (pose == nullptr) {
        throw std::runtime_error("pose is nullptr");
      }
    }
    auto ePose{Eigen::Affine3d::Identity()};
    if (pose != nullptr) {
      std::int32_t dataNumber{16};
      double poseData[dataNumber];
      memcpy(poseData, pose, dataNumber * sizeof(double));

      ePose.matrix() =
          Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>>(poseData);
      // std::cout << ePose.matrix() << std::endl;
    }

    // for (size_t i = 0; i < 10; i++) {
    //   std::cout << "cls: " << *(data.data[0].cls + i) << std::endl;
    //   std::cout << "reg: " << *(data.data[0].reg + i) << std::endl;
    // }

    auto const &jRet{mapEvaluateAdapter.RunDecodeAndPost(data, ePose)};
    if (jRet.length() > mapEvaluateAdapter.MapMsgLength()) {
      throw std::runtime_error(
          "Map Message length=" +
          std::to_string(mapEvaluateAdapter.MapMsgLength()) +
          " is short than map length=" + std::to_string(jRet.length()));
    }

    strcpy(map, jRet.c_str());

#if 0
    auto const &saveMapRoot{"decodeResult/map"};
    auto const &savePoseRoot{"decodeResult/pose"};

    if (!std::filesystem::exists(saveMapRoot)) {
      std::filesystem::create_directories(saveMapRoot);
    }
    if (!std::filesystem::exists(savePoseRoot)) {
      std::filesystem::create_directories(savePoseRoot);
    }
    auto const &saveMapFile{
        std::filesystem::path(saveMapRoot) /
        std::filesystem::path(std::to_string(data.timestamp) + ".json")};

    auto const &savePoseFile{
        std::filesystem::path(savePoseRoot) /
        std::filesystem::path(std::to_string(data.timestamp) + ".txt")};

    std::ofstream stream{saveMapFile};
    stream << jRet;

    write_matrix(ePose.matrix(), savePoseFile);
    std::cout << "[Warrning]" << " saving decode result now!!!" << std::endl;
#endif  // 0

    iRet = 0;
  } catch (const std::exception &e) {
    errMsg = "RunMapDecodeAndPost error, " + std::string(e.what());
  } catch (...) {
    errMsg = "RunMapDecodeAndPost unexpected";
  }
  strcpy(err, errMsg.c_str());
  return iRet;
}

MapEvaluateAdapter::MapEvaluateAdapter() : is_init_{false}, map_ptr_{nullptr} {}
MapEvaluateAdapter::~MapEvaluateAdapter() {}

MapEvaluateAdapter &MapEvaluateAdapter::GetInstance() noexcept {
  static MapEvaluateAdapter instance;
  return instance;
}

void MapEvaluateAdapter::Init(std::string const &jSettings) noexcept(false) {
  auto initFun{[this, &jSettings]() {
    if (map_ptr_ == nullptr) {
      map_ptr_ = std::make_shared<Map>();
    }
    parse_settings(jSettings);
    decoder_.Init(decoder_para_);
    filter_.Init(filter_para_);
    is_init_ = true;
  }};
  std::call_once(init_flag_, initFun);
}

std::string MapEvaluateAdapter::RunDecodeAndPost(
    MapHeadDataRaw const &dataRaw,
    Eigen::Affine3d const &pose) noexcept(false) {
  if (!IsInit()) {
    throw std::runtime_error("MapEvaluateAdapter is not init");
  }

  memset(map_ptr_.get(), 0, sizeof(Map));

  map_ptr_->timestamp = dataRaw.timestamp;
  for (std::int32_t i = 0; i < kMaxMapHeadDatumNumber; ++i) {
    auto const &data{dataRaw.data[i]};
    decoder_.Decode(data.cls, data.reg, i, 0, 0, *map_ptr_);
  }

  if (is_map_filter_enable_) {
    filter_(*map_ptr_, pose);
  }

  auto const &jvMap{boost::json::value_from(*map_ptr_)};
  auto const &strJvMap{boost::json::serialize(jvMap)};

  return strJvMap;
}

void MapEvaluateAdapter::parse_map_decode_para(
    boost::json::value const &jv,
    autodrive::perception::MapDecoder::MapDecoderParameter
        &para) noexcept(false) {
  auto const &obj = jv.as_object();

  std::string name{"preds_number"};
  parse_value(obj, name, para.preds_number);

  name = "down_size";
  parse_value(obj, name, para.down_size);

  name = "net_range";
  parse_value(obj, name, para.net_range);

  name = "detection_range";
  parse_value(obj, name, para.detection_range);

  name = "voxel_size";
  parse_value(obj, name, para.voxel_size);

  name = "type_offset";
  parse_value(obj, name, para.type_offset);

  name = "multi_line";
  parse_value(obj, name, para.multi_line);

  name = "min_pts_num";
  parse_value(obj, name, para.min_pts_num);

  name = "merge_threshold";
  parse_value(obj, name, para.merge_threshold);

  name = "start_cls_threshold";
  parse_value(obj, name, para.start_cls_threshold);

  name = "cls_threshold";
  parse_value(obj, name, para.cls_threshold);

  name = "vec_threshold";
  parse_value(obj, name, para.vec_threshold);

  name = "dis_threshold";
  parse_value(obj, name, para.dis_threshold);

  name = "keypoint_threshold";
  parse_value(obj, name, para.keypoint_threshold);

  name = "dis_loss_weight";
  parse_value(obj, name, para.dis_loss_weight);
}

void MapEvaluateAdapter::parse_map_filter_para(
    boost::json::value const &jv,
    autodrive::perception::MapFilter::MapFilterParameter
        &para) noexcept(false) {
  auto const &obj = jv.as_object();

  std::string name{"roi"};
  parse_value(obj, name, para.roi);

  name = "match_paramemter";
  parse_value(obj, name, para.match_paramemter);

  name = "fusion_parameter";
  parse_value(obj, name, para.fusion_parameter);

  name = "lifetime_threshold";
  parse_value(obj, name, para.lifetime_threshold);

  name = "lifetime_bonus_threshold";
  parse_value(obj, name, para.lifetime_bonus_threshold);
}

void MapEvaluateAdapter::parse_settings(std::string const &jSettings) noexcept(
    false) {
  boost::json::error_code ec;
  auto jv{boost::json::parse(jSettings, ec)};
  if (ec.value() != 0) {
    throw std::runtime_error("parse json string fail: " + ec.message());
  }
  if (!jv.is_array() || jv.as_array().size() < 1) {
    throw std::runtime_error("illegal json string");
  }
  auto const &ja{jv.as_array()};
  auto joRoot{ja.at(0).as_object()};

  std::string const &mapDecoderParaName{"map_decoder_para"};
  if (!joRoot.contains(mapDecoderParaName)) {
    throw std::runtime_error("no " + mapDecoderParaName + " in json");
  } else {
    parse_map_decode_para(joRoot.at(mapDecoderParaName), decoder_para_);
  }

  auto const &mapFilterParaName{"map_filter_para"};
  if (!joRoot.contains(mapFilterParaName)) {
    throw std::runtime_error("no map_filter_para in json");
  } else {
    parse_map_filter_para(joRoot.at(mapFilterParaName), filter_para_);
  }

  auto const &mapMsgLengthName{"map_msg_length"};
  parse_value(joRoot, mapMsgLengthName, map_msg_length_);

  auto const &mapFilterEnableName{"is_map_filter_enable"};
  std::int32_t mapFilterEnableValue;
  parse_value(joRoot, mapFilterEnableName, mapFilterEnableValue);
  if (mapFilterEnableValue == 0) {
    is_map_filter_enable_ = false;
  } else if (mapFilterEnableValue == 1) {
    is_map_filter_enable_ = true;
  } else {
    throw std::runtime_error("illegal is_map_filter_enable value: " +
                             std::to_string(mapFilterEnableValue));
  }
}
}  // namespace perception
}  // namespace autodrive